//
// Created by 刘洋 on 2024/3/2.
//
#include "InlineCurrent.h"
LowPassFilter CurrentFilter;
uint16_t raw_adc[2];
float readADCVoltageInline(CurrSense* sensor,uint8_t ChennalNumber)
{
    HAL_ADC_Start_DMA(sensor->hadc,(uint32_t *)raw_adc,sizeof (raw_adc)/sizeof (raw_adc[0]));
    return raw_adc[ChennalNumber]*_ADC_CONV;
}
void CurrSenseCalibrateOffsets(CurrSense* sensor)
{
    const int calibration_rounds = 1000;

    // 查找0电流时候的电压
    sensor->offset_ia = 0;
    sensor->offset_ib = 0;
    sensor->offset_ic = 0;
    // 读数1000次
    for (int i = 0; i < calibration_rounds; i++) {
        if(_isset(sensor->pinA)) sensor->offset_ia += readADCVoltageInline(sensor,0);
        if(_isset(sensor->pinB)) sensor->offset_ib += readADCVoltageInline(sensor,1);
        if(_isset(sensor->pinC)) sensor->offset_ic += readADCVoltageInline(sensor,1);
        delay(1);
    }
    // 求平均，得到误差
    if(_isset(sensor->pinA)) sensor->offset_ia = sensor->offset_ia / calibration_rounds;
    if(_isset(sensor->pinB)) sensor->offset_ib = sensor->offset_ib / calibration_rounds;
    if(_isset(sensor->pinC)) sensor->offset_ic = sensor->offset_ic / calibration_rounds;
}
void CurrSenseInit(CurrSense* sensor,ADC_HandleTypeDef *hadc,float R_value,float sensor_gain,int pinAIsDef,int pinBIsDef,int pinCIsDef,int sensorDir_A,int sensorDir_B,int sensorDir_C)
{
    sensor->_shunt_resistor = R_value;
    sensor->amp_gain = sensor_gain;
    sensor->volts_to_amps_ratio = 1.0f /sensor->_shunt_resistor / sensor->amp_gain; // volts to amps

    sensor->pinA = pinAIsDef;
    sensor->pinB = pinBIsDef;
    sensor->pinC = pinCIsDef;

    // gains for each phase
    sensor->gain_a = sensor->volts_to_amps_ratio*sensorDir_A;
    sensor->gain_b = sensor->volts_to_amps_ratio*sensorDir_B;
    sensor->gain_c = sensor->volts_to_amps_ratio*sensorDir_C;
    sensor->hadc = hadc;
    CurrSenseCalibrateOffsets(sensor);
    LowPassFilterInit(&CurrentFilter,0.05);
}
void getPhaseCurrents(CurrSense* sensor)
{
     sensor->current_a = (!_isset(sensor->pinA)) ? 0 : (readADCVoltageInline(sensor,0) - sensor->offset_ia)*sensor->gain_a; // amps
     sensor->current_b = (!_isset(sensor->pinB)) ? -(sensor->current_a+sensor->current_c) : (readADCVoltageInline(sensor,1) - sensor->offset_ib)*sensor->gain_b; // amps
     sensor->current_c = (!_isset(sensor->pinC)) ? 0 : (readADCVoltageInline(sensor,1) - sensor->offset_ic)*sensor->gain_c; // amps
}
